#ifndef DRIVETRAIN_H
#define DRIVETRAIN_H
#include "stepperMotor.h"
class Drivetrain
{
public:
    Drivetrain(TimerQueue& timer)
        :currentBearing(0), targetBearing(0), speed(0),
          right(timer,9,8,7,6,true), left(timer,10,11,12,13,true)
    {}
    void setCurrentBearing(int bearing)
    {
        currentBearing = bearing;
        if(currentBearing == targetBearing)
        {
            left.setMotorSpeed(speed);
            right.setMotorSpeed(speed);
        }
        else
        {
            setTargetBearing(targetBearing);
        }
    }//release stopped wheel when currentBearing==targetBearing
    void setTargetBearing(int bearing)//stop one wheel from turning( set!0 speed to another!)
    {
        Serial.print("target: ");Serial.print(bearing);Serial.print('\t');
        targetBearing = bearing;
        if(targetBearing != currentBearing)
        {
            if((currentBearing < 180 &&
                (targetBearing < currentBearing ||
                 targetBearing > (currentBearing +180)%360))||
               (currentBearing >= 180 &&
                (targetBearing < currentBearing &&
                 targetBearing > currentBearing - 180)))
            {
                left.setMotorSpeed(speed/2);
                right.setMotorSpeed(speed);
                Serial.print("left: ");Serial.print(speed/2);Serial.print('\t');
                Serial.print("right: ");Serial.print(speed);Serial.print('\t');
            }//left
            else
            {
                right.setMotorSpeed(speed/2);
                left.setMotorSpeed(speed);
                Serial.print("left: ");Serial.print(speed);Serial.print('\t');
                Serial.print("right: ");Serial.print(speed/2);Serial.print('\t');

            }
        }
    }
    void turnleft(){setTargetBearing((targetBearing+359)%360);}
    void turnright(){setTargetBearing((targetBearing+1)%360);}
    void setSpeed(int percent)
    {
        if(speed<=100)
        {
            left.setMotorSpeed(percent);
            right.setMotorSpeed(percent);
        }
    }
    void stop(){left.stopMotor();right.stopMotor();}
    void faster(){setSpeed(++speed);}
    void slower(){setSpeed(--speed);}
private:
    int currentBearing, targetBearing;
    unsigned int speed;
    StepperMotor left;
    StepperMotor right;

};
#endif // DRIVETRAIN_H
